#include "widget.h"
#include "../resources/ui_widget.h"

Widget::Widget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::Widget)
{
    ui->setupUi(this);
    initCamera();
}

void Widget::grabCameraFrame(const QVideoFrame& buffer)
{
    QImage img;
    QVideoFrame frame(buffer);  // make a copy we can call map (non-const) on
    frame.map(QAbstractVideoBuffer::ReadOnly);
    QImage::Format imageFormat = QVideoFrame::imageFormatFromPixelFormat(
                frame.pixelFormat());
    if (imageFormat != QImage::Format_Invalid) {
        img = QImage(frame.bits(),
                     frame.width(),
                     frame.height(),
                     imageFormat);
    } else {
        // e.g. JPEG
        int nbytes = frame.mappedBytes();
        img = QImage::fromData(frame.bits(), nbytes);
    }
    frame.unmap();
    img = img.mirrored(false, false);

    if(functor){
        img = functor(img);
    }
    filterViewFinder->showImage(img);
}

void Widget::initCamera()
{
    m_camera.clear();
    foreach (QCameraInfo info, QCameraInfo::availableCameras()) {
        m_camera.append(info);
    }

    camera=new QCamera(m_camera.at(0));     //USB相机ID: 0或1

    QCameraViewfinderSettings set;
    set.setResolution(640, 480);//640480
    camera->setViewfinderSettings(set);

    // 用于捕获帧
    filter = new myvideosurface(this);
    connect(filter, &myvideosurface::frameAvailable, this, &Widget::grabCameraFrame, Qt::DirectConnection);
    filterViewFinder = new opencamera(this);
    filterViewFinder->setGeometry(0, 0, 1024, 768);

    camera->setViewfinder(filter);

    ui->tips->setParent(filterViewFinder);
    ui->tips_bottom->setParent(filterViewFinder);
    ui->tips_centre->setParent(filterViewFinder);
    ui->videoWidget = filterViewFinder;

    camera->setCaptureMode(QCamera::CaptureStillImage);
    camera->start();
}

void Widget::switchPage(whichUi which)
{
    switch(which)
    {
    case U1:{
        // 停止处理帧
        functor = nullptr;
        break;
    }
    case U2:{
        // 接管帧处理（with轴线）
      //  functor = [&](QImage img){
        //    img = video_stream_push_axisline(img);
          //  return img;
          functor = nullptr;
           break;
        };
       // break;
    
    case U3:{
        // 接管帧处理
        functor = [&](QImage img){
            img = video_stream_push(img);
            return img;
        };
        break;
    }
    }
}

void Widget::UIsetText(QString str_1, QString str_2){
    ui->tips->setText(str_1);
    ui->tips_bottom->setText(str_2);
    sleep_clock = 0;
}

void Widget::UIsetText_top(QString str){
    ui->tips->setText(str);
}

void Widget::UIsetText_centre(QString str){
    ui->tips_centre->setText(str);
}


//显示处理后的视频流
QImage Widget::video_stream_push_axisline(QImage img){
    //在functor函数中直接处理帧
    QImage cameraGet = img;  
    image_usbcam = img.convertToFormat(QImage::Format_RGB888); //将QImage格式转为Format_RGB888

    // //画轴线
    // double axisline[2][2][2]= {{{(x[0]+x[1])/2,(y[0]+y[1])/2},{(x[2]+x[3])/2,(y[2]+y[3])/2}},{{(x[1]+x[2])/2,(y[1]+y[2])/2},{(x[3]+x[0])/2,(y[3]+y[0])/2}}};
    // double start_inline[2][2] = {{(axisline[0][0][0]+axisline[0][1][0])/2,(axisline[0][0][1]+axisline[0][1][1])/2},{(axisline[1][0][0]+axisline[1][1][0])/2,(axisline[1][0][1]+axisline[1][1][1])/2}};
    // double point_inline[4][2] = {{256,256},{256,256},{256,256},{256,256}}; 
    // int loop = 0; int outcheck = 0;
    // while(outcheck < 4){
    //     outcheck = 0;
    //     for(int j=0; j<2; j++){
    //         for(int p=0; p<2; p++){
    //             for(int k=0; k<2; k++){
    //                 point_inline[j*2+p][k] = start_inline[j][k] + (axisline[j][p][k] - start_inline[j][k])*0.05*loop;
    //             }
    //             if(point_inline[j*2+p][0]>1 && point_inline[j*2+p][1]>1 && point_inline[j*2+p][0] < 639 && point_inline[j*2+p][1] < 479){
    //                 cameraGet.setPixel(int(point_inline[j*2+p][0]), int(point_inline[j*2+p][1]), qRgb(0, 255, 0));
    //                 // cameraGet.setPixel(int(point_inline[j*2+p][0])+1, int(point_inline[j*2+p][1])+1, qRgb(0, 255, 0));
    //             }else{
    //                 outcheck++;
    //             }
    //         } 
    //     }
    //     loop++;
    // }

    //画轴线
    double central_point[2] = {(x[0]+x[1]+x[2]+x[3])/4,(y[0]+y[1]+y[2]+y[3])/4};
    double point_inline[2][2] = {{central_point[0],256},{central_point[0],256}};
    int loop = 0; int outcheck = 0;
    while(outcheck < 2){
        outcheck = 0;
        point_inline[0][1] = central_point[1] + loop;
        point_inline[1][1] = central_point[1] - loop;
        for(int i=0; i<2; i++){
            if(point_inline[i][1]>1 && point_inline[i][1]<479){
                cameraGet.setPixel(int(point_inline[i][0]), int(point_inline[i][1]), qRgb(0, 255, 0));
                cameraGet.setPixel(int(point_inline[i][0])+1, int(point_inline[i][1]), qRgb(0, 255, 0));
                cameraGet.setPixel(int(point_inline[i][0])-1, int(point_inline[i][1]), qRgb(0, 255, 0));
            }else{outcheck++;}
        }
        loop++;
    }

    // //对应Apriltag的二维码可视化
    // if(apriltags_realtimepaint[0] != 0){

    //     QRgb color_value[4] = {qRgb(255, 0, 0),qRgb(0, 0, 255),qRgb(0, 255, 0),qRgb(255, 128, 0)};

    //     //实时监测角点
    //     double x_now[4][4] = {{apriltags_realtimepaint[0], apriltags_realtimepaint[2], apriltags_realtimepaint[4], apriltags_realtimepaint[6]},
    //                         {apriltags_realtimepaint[8], apriltags_realtimepaint[10], apriltags_realtimepaint[12], apriltags_realtimepaint[14]},
    //                         {apriltags_realtimepaint[16], apriltags_realtimepaint[18], apriltags_realtimepaint[20], apriltags_realtimepaint[22]},
    //                         {apriltags_realtimepaint[24], apriltags_realtimepaint[26], apriltags_realtimepaint[28], apriltags_realtimepaint[30]}};
    //     double y_now[4][4] = {{apriltags_realtimepaint[1], apriltags_realtimepaint[3], apriltags_realtimepaint[5], apriltags_realtimepaint[7]},
    //                         {apriltags_realtimepaint[9], apriltags_realtimepaint[11], apriltags_realtimepaint[13], apriltags_realtimepaint[15]},
    //                         {apriltags_realtimepaint[17], apriltags_realtimepaint[19], apriltags_realtimepaint[21], apriltags_realtimepaint[23]},
    //                         {apriltags_realtimepaint[25], apriltags_realtimepaint[27], apriltags_realtimepaint[29], apriltags_realtimepaint[31]}};

    //     //画角点
    //     for(int i=0; i<4; i++){
    //         for(int offset_x=-1; offset_x<2; offset_x++){
    //             for(int offset_y=-1; offset_y<2; offset_y++){
    //                 cameraGet.setPixel(int(x[i])+offset_x, int(y[i])+offset_y, color_value[i]);
    //                 for(int cornum=0; cornum<4; cornum++){
    //                     if(x_now[cornum][0] == -1){continue;}
    //                     cameraGet.setPixel(int(x_now[cornum][i])+offset_x, int(y_now[cornum][i])+offset_y, color_value[i]);
    //                 }
    //             }
    //         }
    //     }

    //     //画边线
    //     for(int p=0; p<5; p++){
    //         for(int i=0; i<4; i++){
    //             double point_start[10] = {x[i], y[i], x_now[0][i], y_now[0][i], x_now[1][i], y_now[1][i], x_now[2][i], y_now[2][i], x_now[3][i], y_now[3][i]};
    //             if(point_start[2*p] == -1){continue;}   
    //             double point_end[10] = {x[(i+1)%4], y[(i+1)%4], x_now[0][(i+1)%4], y_now[0][(i+1)%4], x_now[1][(i+1)%4], y_now[1][(i+1)%4], x_now[2][(i+1)%4], y_now[2][(i+1)%4], x_now[3][(i+1)%4], y_now[3][(i+1)%4]};
    //             double delta_x[5] = {point_end[0] - point_start[0], point_end[2] - point_start[2], point_end[4] - point_start[4], point_end[6] - point_start[6], point_end[8] - point_start[8]};
    //             double delta_y[5] = {point_end[1] - point_start[1], point_end[3] - point_start[3], point_end[5] - point_start[5], point_end[7] - point_start[7], point_end[9] - point_start[9]};
    //             int line_dense = 20;        //边线密度
    //             for(int loop=0; loop<line_dense; loop++){
    //                 cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p]), int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p]), color_value[i]);
    //                 cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p])+1, int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p])+1, color_value[i]);
    //                 cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p])-1, int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p])-1, color_value[i]);
    //             }
    //         }
    //     }
    // }
    return cameraGet;
}


//显示处理后的视频流
QImage Widget::video_stream_push(QImage img){
    //在functor函数中直接处理帧
    QImage cameraGet = img;  
    image_usbcam = img.convertToFormat(QImage::Format_RGB888); //将QImage格式转为Format_RGB888

    //对应Apriltag的二维码可视化
    if(apriltags_realtimepaint[0] != 0){

        QRgb color_value[4] = {qRgb(255, 0, 0),qRgb(0, 0, 255),qRgb(0, 255, 0),qRgb(255, 128, 0)};

        //实时监测角点
        double x_now[4][4] = {{apriltags_realtimepaint[0], apriltags_realtimepaint[2], apriltags_realtimepaint[4], apriltags_realtimepaint[6]},
                            {apriltags_realtimepaint[8], apriltags_realtimepaint[10], apriltags_realtimepaint[12], apriltags_realtimepaint[14]},
                            {apriltags_realtimepaint[16], apriltags_realtimepaint[18], apriltags_realtimepaint[20], apriltags_realtimepaint[22]},
                            {apriltags_realtimepaint[24], apriltags_realtimepaint[26], apriltags_realtimepaint[28], apriltags_realtimepaint[30]}};
        double y_now[4][4] = {{apriltags_realtimepaint[1], apriltags_realtimepaint[3], apriltags_realtimepaint[5], apriltags_realtimepaint[7]},
                            {apriltags_realtimepaint[9], apriltags_realtimepaint[11], apriltags_realtimepaint[13], apriltags_realtimepaint[15]},
                            {apriltags_realtimepaint[17], apriltags_realtimepaint[19], apriltags_realtimepaint[21], apriltags_realtimepaint[23]},
                            {apriltags_realtimepaint[25], apriltags_realtimepaint[27], apriltags_realtimepaint[29], apriltags_realtimepaint[31]}};

        //画角点
       
        for(int i=0; i<4; i++){
            for(int offset_x=-1; offset_x<2; offset_x++){
                for(int offset_y=-1; offset_y<2; offset_y++){
                    cameraGet.setPixel(int(x[i])+offset_x, int(y[i])+offset_y, color_value[i]);
                    for(int cornum=0; cornum<4; cornum++){
                        if(x_now[cornum][0] == -1){continue;}
                        cameraGet.setPixel(int(x_now[cornum][i])+offset_x, int(y_now[cornum][i])+offset_y, color_value[i]);
                    }
                }
            }
        }

        //画边线
        for(int p=0; p<5; p++){
            for(int i=0; i<4; i++){
                double point_start[10] = {x[i], y[i], x_now[0][i], y_now[0][i], x_now[1][i], y_now[1][i], x_now[2][i], y_now[2][i], x_now[3][i], y_now[3][i]};
                double point_end[10] = {x[(i+1)%4], y[(i+1)%4], x_now[0][(i+1)%4], y_now[0][(i+1)%4], x_now[1][(i+1)%4], y_now[1][(i+1)%4], x_now[2][(i+1)%4], y_now[2][(i+1)%4], x_now[3][(i+1)%4], y_now[3][(i+1)%4]};
                double delta_x[5] = {point_end[0] - point_start[0], point_end[2] - point_start[2], point_end[4] - point_start[4], point_end[6] - point_start[6], point_end[8] - point_start[8]};
                double delta_y[5] = {point_end[1] - point_start[1], point_end[3] - point_start[3], point_end[5] - point_start[5], point_end[7] - point_start[7], point_end[9] - point_start[9]};
                if(point_start[2*p] == -1){continue;}
                int line_dense = 20;        //边线密度
                for(int loop=0; loop<line_dense; loop++){
                    cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p]), int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p]), color_value[i]);
                    cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p])+1, int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p])+1, color_value[i]);
                    cameraGet.setPixel(int(point_start[2*p] + ((double)loop/line_dense)*delta_x[p])-1, int(point_start[1+2*p] + ((double)loop/line_dense)*delta_y[p])-1, color_value[i]);
                }
            }
        }
    }
  
    return cameraGet;
}

Widget::~Widget()
{
    delete ui;
   // delete filter;
   // delete camera;
   //delete filterViewFinder;
}
